#ifndef __SV2_ARUCO_DET__
#define __SV2_ARUCO_DET__

#include "sms_core.h"
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/tracking.hpp>
#include <string>
#include <chrono>
#include <mutex>
#include <queue>
#include <condition_variable>


/*
依赖安装
sudo apt install libopencv-dev

*/
#define BUFF_SIZE 30


namespace sv2 {

class ImgMsg {
  public:
    ImgMsg(cv::Mat img_, nlohmann::json msg_) : img(img_), msg(msg_) {}
    cv::Mat img;
    nlohmann::json msg;
};

class AutoFocusDetCppNode : public sms::BaseNode
{
public:
  AutoFocusDetCppNode(
    std::string job_name,
    std::string param_file,
    std::string ip="127.0.0.1",
    int port=9094,
    nlohmann::json kwargs = {}
  ) : sms::BaseNode("AutoFocusDetCppNode", job_name, param_file, ip, port, kwargs), 
    _img_sub("/" + job_name + "/sensor/image_raw", "std_msgs::Null", std::bind(&AutoFocusDetCppNode::img_callback, this, std::placeholders::_1)),
    _global_img_pub(nullptr),
    _local_img_pub(nullptr),
    _global_res_sub(nullptr),
    _local_res_sub(nullptr),
    _res_pub("/" + job_name + "/detector/results", "spirecv_msgs::2DTargets"),
    _res_img_pub("/" + job_name + "/detector/image_results", "memory_msgs::RawImage")
  {
    // 读取节点参数
    this->_global_job_name = this->get_param("global_job_name", "global_det");
    this->_local_job_name = this->get_param("local_job_name", "local_det");
    this->_lock_thres = this->get_param("lock_thres", 5);
    this->_unlock_thres = this->get_param("unlock_thres", 5);
    this->_lock_scale_init = this->get_param("lock_scale_init", 12.0);
    this->_lock_scale = this->get_param("lock_scale", 10.0);
    this->_keep_unlocked = this->get_param("keep_unlocked", true);
    this->_use_square_region = this->get_param("use_square_region", false);
    nlohmann::json categories_filter = this->get_param("categories_filter", {});
    this->params_help();

    /*
    logger.info("lock_thres: " + std::to_string(this->_lock_thres));
    logger.info("unlock_thres: " + std::to_string(this->_unlock_thres));
    logger.info("lock_scale_init: " + std::to_string(this->_lock_scale_init));
    logger.info("lock_scale: " + std::to_string(this->_lock_scale));
    logger.info("keep_unlocked: " + std::to_string(this->_keep_unlocked));
    logger.info("use_square_region: " + std::to_string(this->_use_square_region));
    logger.info("categories_filter: " + categories_filter.dump());
    */

    this->_img_id = 0;
    this->_n_imgs = 0;
    this->_nonroi_count = 0;
    this->_imgs.resize(BUFF_SIZE);

    this->_last_time = sms::get_time_sec();
    this->_global_img_pub = new sms::Publisher("/" + this->_global_job_name + "/sensor/image_raw", "memory_msgs::RawImage");
    this->_local_img_pub = new sms::Publisher("/" + this->_local_job_name + "/sensor/image_raw", "memory_msgs::RawImage");
    this->_global_res_sub = new sms::Subscriber(
      "/" + this->_global_job_name + "/detector/results", 
      "spirecv_msgs::2DTargets",
      std::bind(&AutoFocusDetCppNode::global_res_callback, this, std::placeholders::_1)
    );
    this->_local_res_sub = new sms::Subscriber(
      "/" + this->_local_job_name + "/detector/results", 
      "spirecv_msgs::2DTargets",
      std::bind(&AutoFocusDetCppNode::local_res_callback, this, std::placeholders::_1)
    );

    // 将读取到的参数转换到成员变量：_categories_filter
    for (const auto& item : categories_filter) { this->_categories_filter.push_back(item); }
  }
  ~AutoFocusDetCppNode()
  {
  }
  void run();

  void img_callback(nlohmann::json msg)
  {
    // std::cout <<"this->_n_imgs: "<< this->_n_imgs << std::endl;
    if (this->_n_imgs < 6 || sms::get_time_sec() - this->_last_time > 0.1)
    {
      this->_imgs[this->_img_id] = sms::sms2cvimg(msg);
      // std::cout <<"this->_locked: "<< this->_locked << std::endl;
      if (this->_locked)
      {
        bool has_roi = false;
        nlohmann::json img_msg = this->_local_img_pub->cvimg2sms_mem(this->_imgs[this->_img_id]);
        {
          std::lock_guard<std::mutex> lock(this->_ctrl_msg_mtx);
          if (this->_ctrl_msg["rois"].size() > 0)
          {
            img_msg["rois"] = this->_ctrl_msg["rois"];
            // this->_ctrl_msg["rois"].clear();
            has_roi = true;
          }
        }
        // std::cout <<"has_roi: "<< has_roi << std::endl;
        if (has_roi)
        {
          img_msg["img_id"] = this->_img_id;
          this->_local_img_pub->publish(img_msg);
          this->_nonroi_count = 0;
        }
        else
        {
          this->_nonroi_count ++;
          if (this->_nonroi_count >= 5)
          {
            this->_unlock_count = 0;
            this->_lock_scale_count = -1;
            this->_locked = false;
            this->_last_cx = -1.f;
            this->_last_cy = -1.f;
            this->_lock_rois.clear();
          }
        }
      }
      else
      {
        nlohmann::json img_msg = this->_global_img_pub->cvimg2sms_mem(this->_imgs[this->_img_id]);
        img_msg["img_id"] = this->_img_id;
        this->_global_img_pub->publish(img_msg);
      }
      this->_img_id ++;
      if (this->_img_id >= BUFF_SIZE)  this->_img_id = 0;
      this->_last_time = sms::get_time_sec();
      this->_n_imgs ++;
    }
  }

  void global_res_callback(nlohmann::json msg)
  {
    if (!this->_locked)
    {
      {
        std::lock_guard<std::mutex> lock(this->_global_queue_mtx);
        this->_global_queue.push(msg);
      }
      // this->_global_cv.notify_one();
    }
  }

  void local_res_callback(nlohmann::json msg)
  {
    if (this->_locked)
    {
      {
        std::lock_guard<std::mutex> lock(this->_local_queue_mtx);
        this->_local_queue.push(msg);
      }
      // this->_local_cv.notify_one();
    }
  }

private:
  std::vector<std::string> _categories_filter;
  bool _keep_unlocked;
  bool _use_square_region;

  bool _locked = false;
  int _lock_thres;
  int _unlock_thres;
  int _lock_count;
  int _unlock_count;
  int _nonroi_count;
  
  float _lock_scale_init;
  float _lock_scale;
  int _lock_scale_count;

  std::vector<std::vector<int> > _lock_rois;
  float _last_cx;
  float _last_cy;
  float _unlock_last_cx;
  float _unlock_last_cy;
  double _last_time;

  std::string _global_job_name;
  std::string _local_job_name;
  std::vector<cv::Mat> _imgs;
  int _img_id;
  int _n_imgs;
  nlohmann::json _ctrl_msg;
  std::mutex _ctrl_msg_mtx;

  // 订阅话题
  sms::Subscriber _img_sub;
  sms::Subscriber* _global_res_sub;
  sms::Subscriber* _local_res_sub;
  // 发布话题
  sms::Publisher* _global_img_pub;
  sms::Publisher* _local_img_pub;
  sms::Publisher _res_pub;
  sms::Publisher _res_img_pub;

  std::mutex _global_queue_mtx;
  std::queue<nlohmann::json> _global_queue;
  std::condition_variable _global_cv;

  std::mutex _local_queue_mtx;
  std::queue<nlohmann::json> _local_queue;
  std::condition_variable _local_cv;

};


}
#endif
